
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_imu.h
  * @author     baiyang
  * @date       2021-10-11
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include "sensor_imu_backend.h"

#include <common/gp_config.h>
#include <common/gp_rotations.h>
#include <common/filter/lpfilter.h>
#include <common/gp_math/light_matrix.h>
/*-----------------------------------macro------------------------------------*/
// Gyro and Accelerometer calibration criteria
#define AP_INERTIAL_SENSOR_ACCEL_TOT_MAX_OFFSET_CHANGE  4.0f
#define AP_INERTIAL_SENSOR_ACCEL_MAX_OFFSET             250.0f
#define AP_INERTIAL_SENSOR_ACCEL_VIBE_FLOOR_FILT_HZ     5.0f    // accel vibration floor filter hz
#define AP_INERTIAL_SENSOR_ACCEL_VIBE_FILT_HZ           2.0f    // accel vibration filter hz
#define AP_INERTIAL_SENSOR_ACCEL_PEAK_DETECT_TIMEOUT_MS 500     // peak-hold detector timeout

/**
   maximum number of INS instances available on this platform. If more
   than 1 then redundant sensors may be available
 */
#ifndef INS_MAX_INSTANCES
#define INS_MAX_INSTANCES 3
#endif

#define INS_MAX_BACKENDS  2*INS_MAX_INSTANCES
#define INS_MAX_NOTCHES 4

#ifndef INS_VIBRATION_CHECK_INSTANCES
    #define INS_VIBRATION_CHECK_INSTANCES INS_MAX_INSTANCES
#endif

#define XYZ_AXIS_COUNT    3

// The maximum we need to store is gyro-rate / loop-rate, worst case ArduCopter with BMI088 is 2000/400
#define INS_MAX_GYRO_WINDOW_SAMPLES 8

#define DEFAULT_IMU_LOG_BAT_MASK 0

#ifndef HAL_INS_TEMPERATURE_CAL_ENABLE
#define HAL_INS_TEMPERATURE_CAL_ENABLE 0
#endif

/*----------------------------------typedef-----------------------------------*/
enum Gyro_Calibration_Timing {
    GYRO_CAL_NEVER = 0,
    GYRO_CAL_STARTUP_ONLY = 1
};

// peak hold detector state for primary accel
typedef struct {
    float accel_peak_hold_neg_x;
    uint32_t accel_peak_hold_neg_x_age;
} PeakHoldState;

/**
  * @brief       sensor_imu is an abstraction for gyro and accel measurements 
  *              which are correctly aligned to the body axes and scaled to SI units.
  * @param[out]  
  * @retval      
  * @note        
  */
typedef struct {
    // backend objects
    sensor_imu_backend *_backends[INS_MAX_BACKENDS];

    // number of gyros and accel drivers. Note that most backends
    // provide both accel and gyro data, so will increment both
    // counters on initialisation
    uint8_t _gyro_count;
    uint8_t _accel_count;
    uint8_t _backend_count;

    // the selected loop rate at which samples are made available
    uint16_t _loop_rate;
    float _loop_delta_t;
    float _loop_delta_t_max;
    
    // filtering frequency (0 means default)
    Param_int16    _accel_filter_cutoff;
    Param_int16    _gyro_filter_cutoff;
    Param_int8     _gyro_cal_timing;

    // use for attitude, velocity, position estimates
    Param_int8     _use[INS_MAX_INSTANCES];

    // control enable of fast sampling
    Param_int8     _fast_sampling_mask;

    // control enable of fast sampling
    Param_int8     _fast_sampling_rate;

    // control enable of detected sensors
    Param_int8     _enable_mask;

    // IDs to uniquely identify each sensor: shall remain
    // the same across reboots
    Param_int32 _accel_id[INS_MAX_INSTANCES];
    Param_int32 _gyro_id[INS_MAX_INSTANCES];

    // accelerometer scaling and offsets
    Param_Vector3f _accel_scale[INS_MAX_INSTANCES];
    Param_Vector3f _accel_offset[INS_MAX_INSTANCES];
    Param_Vector3f _gyro_offset[INS_MAX_INSTANCES];

    // accelerometer position offset in body frame
    Param_Vector3f _accel_pos[INS_MAX_INSTANCES];

    // threshold for detecting stillness
    Param_float _still_threshold;

    // Trim options
    Param_int8 _acc_body_aligned;
    Param_int8 _trim_option;

    Vector3f_t _trim_rad;
    bool _new_trim;

    bool _accel_cal_requires_reboot;

    // sensor error count at startup (used to ignore errors within 2 seconds of startup)
    uint32_t _accel_startup_error_count[INS_MAX_INSTANCES];
    uint32_t _gyro_startup_error_count[INS_MAX_INSTANCES];
    bool     _startup_error_counts_set;
    uint32_t _startup_ms;

    // Most recent gyro reading
    Vector3f_t _gyro[INS_MAX_INSTANCES];
    Vector3f_t _delta_angle[INS_MAX_INSTANCES];
    float      _delta_angle_dt[INS_MAX_INSTANCES];
    bool       _delta_angle_valid[INS_MAX_INSTANCES];

    // time accumulator for delta angle accumulator
    float      _delta_angle_acc_dt[INS_MAX_INSTANCES];
    Vector3f_t _delta_angle_acc[INS_MAX_INSTANCES];
    Vector3f_t _last_delta_angle[INS_MAX_INSTANCES];
    Vector3f_t _last_raw_gyro[INS_MAX_INSTANCES];

    // bitmask indicating if a sensor is doing sensor-rate sampling:
    uint8_t _accel_sensor_rate_sampling_enabled;
    uint8_t _gyro_sensor_rate_sampling_enabled;

    // multipliers for data supplied via sensor-rate logging:
    uint16_t _accel_raw_sampling_multiplier[INS_MAX_INSTANCES];
    uint16_t _gyro_raw_sampling_multiplier[INS_MAX_INSTANCES];

    // accelerometer max absolute offsets to be used for calibration
    float _accel_max_abs_offsets[INS_MAX_INSTANCES];

    // accelerometer and gyro raw sample rate in units of Hz
    float  _accel_raw_sample_rates[INS_MAX_INSTANCES];
    float  _gyro_raw_sample_rates[INS_MAX_INSTANCES];

    // per-sensor orientation to allow for board type defaults at runtime
    enum RotationEnum _gyro_orientation[INS_MAX_INSTANCES];
    enum RotationEnum _accel_orientation[INS_MAX_INSTANCES];

    // vibration and clipping
    uint32_t _accel_clip_count[INS_MAX_INSTANCES];
    LowPassFilt_vec3f _accel_vibe_floor_filter[INS_VIBRATION_CHECK_INSTANCES];
    LowPassFilt_vec3f _accel_vibe_filter[INS_VIBRATION_CHECK_INSTANCES];

    // last sample time in microseconds. Use for deltaT calculations
    // on non-FIFO sensors
    uint64_t _accel_last_sample_us[INS_MAX_INSTANCES];
    uint64_t _gyro_last_sample_us[INS_MAX_INSTANCES];
    
    // sample times for checking real sensor rate for FIFO sensors
    uint16_t _sample_accel_count[INS_MAX_INSTANCES];
    uint32_t _sample_accel_start_us[INS_MAX_INSTANCES];
    uint16_t _sample_gyro_count[INS_MAX_INSTANCES];
    uint32_t _sample_gyro_start_us[INS_MAX_INSTANCES];

    // how many sensors samples per notify to the backend
    uint8_t _accel_over_sampling[INS_MAX_INSTANCES];
    uint8_t _gyro_over_sampling[INS_MAX_INSTANCES];

    // board orientation from AHRS
    enum RotationEnum _board_orientation;
    float _custom_rotation[3][3];

    uint8_t imu_kill_mask;

    // last time a wait_for_sample() returned a sample
    uint32_t _last_sample_usec;

    // target time for next wait_for_sample() return
    uint32_t _next_sample_usec;

    // time between samples in microseconds
    uint32_t _sample_period_usec;

    // last time update() completed
    uint64_t _last_update_usec;

    // health of gyros and accels
    bool _gyro_healthy[INS_MAX_INSTANCES];
    bool _accel_healthy[INS_MAX_INSTANCES];

    uint32_t _accel_error_count[INS_MAX_INSTANCES];
    uint32_t _gyro_error_count[INS_MAX_INSTANCES];

    // temperatures for an instance if available
    float _temperature[INS_MAX_INSTANCES];

     // Most recent accelerometer reading
    Vector3f_t _accel[INS_MAX_INSTANCES];
    Vector3f_t _delta_velocity[INS_MAX_INSTANCES];
    float _delta_velocity_dt[INS_MAX_INSTANCES];
    bool _delta_velocity_valid[INS_MAX_INSTANCES];

    // delta velocity accumulator
    Vector3f_t _delta_velocity_acc[INS_MAX_INSTANCES];

    // time accumulator for delta velocity accumulator
    float _delta_velocity_acc_dt[INS_MAX_INSTANCES];

    // Low Pass filters for gyro and accel
    LowPassFilt_vec3f _accel_filter[INS_MAX_INSTANCES];
    LowPassFilt_vec3f _gyro_filter[INS_MAX_INSTANCES];
    Vector3f_t _accel_filtered[INS_MAX_INSTANCES];
    Vector3f_t _gyro_filtered[INS_MAX_INSTANCES];

    bool _new_accel_data[INS_MAX_INSTANCES];
    bool _new_gyro_data[INS_MAX_INSTANCES];

    // calibrated_ok/id_ok flags
    bool _gyro_cal_ok[INS_MAX_INSTANCES];
    bool _accel_id_ok[INS_MAX_INSTANCES];

    // primary accel and gyro
    uint8_t _primary_gyro;
    uint8_t _primary_accel;

    // mask of accels and gyros which we will be actively using
    // and this should wait for in wait_for_sample()
    uint8_t _gyro_wait_mask;
    uint8_t _accel_wait_mask;

    // has wait_for_sample() found a sample?
    bool _have_sample;

    bool _backends_detected;

    // are gyros or accels currently being calibrated
    bool _calibrating_accel;
    bool _calibrating_gyro;

    // the delta time in seconds for the last sample
    float _delta_time;

    PeakHoldState _peak_hold_state;
} sensor_imu;
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sensor_imu_ctor();

sensor_imu *sensor_imu_get_singleton();

uint8_t sensor_imu_get_primary_accel(void);
uint8_t sensor_imu_get_primary_gyro(void);

void sensor_imu_init(uint16_t loop_rate);

void sensor_imu_assign_param();
void sensor_imu_gyr_offset_id_variable_to_param();
void sensor_imu_acc_scale_offset_id_variable_to_param();

/*
  update gyro and accel values from backends
 */
void sensor_imu_update(void);

void sensor_imu_wait_for_sample(void);
uint64_t sensor_imu_get_last_update_usec(void);

/*
  register a new gyro instance
 */
bool sensor_imu_register_gyro(uint8_t *instance, uint16_t raw_sample_rate_hz, uint32_t id);

/*
  register a new accel instance
 */
bool sensor_imu_register_accel(uint8_t *instance, uint16_t raw_sample_rate_hz, uint32_t id);

/*
 * Start all backends for gyro and accel measurements. It automatically calls
 * detect_backends() if it has not been called already.
 */
void sensor_imu_start_backends();

/*
  detect available backends for this board
 */
void sensor_imu_detect_backends();

void sensor_imu_init_gyro();

bool sensor_imu_get_delta_angle(uint8_t i, Vector3f_t *delta_angle, float *delta_angle_dt);
bool sensor_imu_get_delta_velocity(uint8_t i, Vector3f_t *delta_velocity, float *delta_velocity_dt);

// return true if we are in a calibration
bool sensor_imu_calibrating();

// output GCS startup messages
bool sensor_imu_get_output_banner(uint8_t backend_id, char* banner, uint8_t banner_len);

// accelerometer clipping reporting
uint32_t sensor_imu_get_accel_clip_count(uint8_t instance);

// get_gyro_health_all - return true if all gyros are healthy
bool sensor_imu_get_gyro_health_all(void);

// gyro_calibration_ok_all - returns true if all gyros were calibrated successfully
bool sensor_imu_gyro_calibrated_ok_all();

// return true if gyro instance should be used (must be healthy and have it's use parameter set to 1)
bool sensor_imu_use_gyro(uint8_t instance);

// get_accel_health_all - return true if all accels are healthy
bool sensor_imu_get_accel_health_all(void);

/*
  calculate the trim_roll and trim_pitch. This is used for redoing the
  trim without needing a full accel cal
 */
bool sensor_imu_calibrate_trim(Vector3f_t *trim_rad);

/*
  check if the accelerometers are calibrated in 3D and that current number of accels matched number when calibrated
 */
bool sensor_imu_accel_calibrated_ok_all();

// return true if accel instance should be used (must be healthy and have it's use parameter set to 1)
bool sensor_imu_use_accel(uint8_t instance);

/* Find the N instance of the backend that has already been successfully detected */
sensor_imu_backend *sensor_imu_find_backend(int16_t backend_id, uint8_t instance);

// calculate vibration levels and check for accelerometer clipping (called by a backends)
void sensor_imu_calc_vibration_and_clipping(uint8_t instance, const Vector3f_t *accel, float dt);

// peak hold detector for slower mechanisms to detect spikes
void sensor_imu_set_accel_peak_hold(uint8_t instance, const Vector3f_t *accel);

// retrieve latest calculated vibration levels
Vector3f_t sensor_imu_get_vibration_levels2(uint8_t instance);
Vector3f_t sensor_imu_get_vibration_levels();

// check for vibration movement. Return true if all axis show nearly zero movement
bool sensor_imu_is_still();

/*
  update IMU kill mask, used for testing IMU failover
 */
void sensor_imu_kill_imu(uint8_t imu_idx, bool kill_it);

float sensor_imu_get_delta_time();

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



